// Unit test for tilt phase model
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <feed_gait/model/tilt_phase/feed_tilt_phase_model.h>
#include <test_utilities/test_eigen.h>
#include <gtest/gtest.h>

// Namespaces
using namespace feed_gait;
using namespace feed_gait::tilt_phase_model;
using namespace testutilities;
using namespace rot_conv;

// Test: fusedDeviationTiltN
TEST(TiltPhaseModelTest, testDeviationTiltPhaseN)
{
	// Test many different cases
	EXPECT_EIGEQQ_UT(Quat(0.9877742528612518, -0.1387157440287830, -0.0711334502392314, 0.0000000000000761), TPMHelper::deviationTiltPhaseN(0.3, 0.2, 0.02, 0.06, 0.08));
	EXPECT_EIGEQQ_UT(Quat(0.8513495292192863, 0.5093396243280424, 0.1256070308042839, -0.0000000000082928), TPMHelper::deviationTiltPhaseN(-1.06, -0.52, 0, -0.24, -0.06));
	EXPECT_EIGEQQ_UT(Quat(0.9672942554046911, -0.0533744603191762, -0.2479778023265821, 0.0000000000092243), TPMHelper::deviationTiltPhaseN(0.13, 0.39, 0.02, -0.11, 0.11));
	EXPECT_EIGEQQ_UT(Quat(0.7047873617098455, -0.6741005977731337, 0.2210501274733383, 0.0000000000225324), TPMHelper::deviationTiltPhaseN(1.48, -0.18, -0.01, 0.32, 0.12));
	EXPECT_EIGEQQ_UT(Quat(0.7296373667268118, 0.2375015930946705, 0.6412661743406223, -0.0000000001396493), TPMHelper::deviationTiltPhaseN(-0.25, -1.13, 0.2, 0.29, 0.18));
	EXPECT_EIGEQQ_UT(Quat(0.6228001721877331, 0.7769886239190438, -0.0916985486434802, -0.0000000003005517), TPMHelper::deviationTiltPhaseN(-1.46, 0.49, 0.3, 0.24, 0.07));
	EXPECT_EIGEQQ_UT(Quat(0.8524262006827968, -0.3182794410669874, -0.4148105227493372, -0.0000000000546951), TPMHelper::deviationTiltPhaseN(0.76, 0.81, 0.11, -0.08, -0.13));
	EXPECT_EIGEQQ_UT(Quat(0.8309184446843785, 0.4806947804972791, -0.2801911245667256, -0.0000000005527613), TPMHelper::deviationTiltPhaseN(-1.32, 0.47, -0.32, -0.16, -0.16));
	EXPECT_EIGEQQ_UT(Quat(0.8187853030022878, -0.1143221316871464, -0.5626020598913222, 0.0000000001176420), TPMHelper::deviationTiltPhaseN(0.61, 1.02, 0.32, -0.13, -0.19));
	EXPECT_EIGEQQ_UT(Quat(0.9392293004848878, 0.2863240297953517, 0.1893855091405323, -0.0000000000531192), TPMHelper::deviationTiltPhaseN(-0.37, -0.19, 0.21, 0.19, -0.13));
	EXPECT_EIGEQQ_UT(Quat(0.9850851910875232, 0.1595069736963143, 0.0645344221505143, 0.0000000000047289), TPMHelper::deviationTiltPhaseN(-0.17, -0.03, 0.15, 0.1, 0.1));
	EXPECT_EIGEQQ_UT(Quat(0.8428197965183729, -0.3711397986977295, 0.3897563859891688, -0.0000000003215073), TPMHelper::deviationTiltPhaseN(0.56, -0.7, -0.24, 0.11, -0.15));
	EXPECT_EIGEQQ_UT(Quat(0.7711851035779954, -0.6365897815601227, -0.0051948082382249, -0.0000000000311639), TPMHelper::deviationTiltPhaseN(1.44, -0.01, 0.06, -0.11, -0.11));
	EXPECT_EIGEQQ_UT(Quat(0.8243693389921597, 0.4123939301498040, -0.3877453278960668, 0.0000000002405563), TPMHelper::deviationTiltPhaseN(-0.77, 0.79, 0.14, 0, 0.16));
	EXPECT_EIGEQQ_UT(Quat(0.6343560771390888, -0.2247029068174922, -0.7396627414331994, 0.0000000002110143), TPMHelper::deviationTiltPhaseN(0.15, 1.44, -0.25, -0.25, -0.1));
	EXPECT_EIGEQQ_UT(Quat(0.8667795978123817, 0.2469217694837343, -0.4332698565227301, -0.0000000003294785), TPMHelper::deviationTiltPhaseN(-0.77, 1.07, -0.18, 0.22, 0.17));
	EXPECT_EIGEQQ_UT(Quat(0.8565669865248692, 0.4996037638118054, 0.1291862097159850, -0.0000000000139507), TPMHelper::deviationTiltPhaseN(-0.95, -0.47, 0.08, -0.17, -0.01));
	EXPECT_EIGEQQ_UT(Quat(0.8405774759059167, -0.5006778776784335, 0.2067635601431193, 0.0000000000645736), TPMHelper::deviationTiltPhaseN(1.04, -0.47, 0.03, 0.06, 0.17));
	EXPECT_EIGEQQ_UT(Quat(0.8163856140692508, -0.4169310825635187, 0.3996035554561354, -0.0000000000881770), TPMHelper::deviationTiltPhaseN(0.81, -0.67, -0.08, 0.18, 0.03));
	EXPECT_EIGEQQ_UT(Quat(0.7010224579331489, 0.4170791412921694, 0.5784570021811068, -0.0000000000118432), TPMHelper::deviationTiltPhaseN(-0.53, -1.33, 0.2, 0.02, 0.17));
	EXPECT_EIGEQQ_UT(Quat(0.7953157399713218, -0.2905427411213329, 0.5320317559465516, -0.0000000000366001), TPMHelper::deviationTiltPhaseN(0.22, -1.16, -0.34, -0.02, -0.07));
	EXPECT_EIGEQQ_UT(Quat(0.7982071208201527, -0.3963789757803403, 0.4535957449440313, -0.0000000000034562), TPMHelper::deviationTiltPhaseN(0.92, -1.06, 0.02, -0.13, -0.13));
	EXPECT_EIGEQQ_UT(Quat(0.9019965220189511, 0.4179136861750212, -0.1083993781039445, -0.0000000001940051), TPMHelper::deviationTiltPhaseN(-0.74, 0.32, 0.13, 0.11, 0.1));
	EXPECT_EIGEQQ_UT(Quat(0.6995009135713848, 0.7144452087514315, 0.0163253056608901, -0.0000000003964129), TPMHelper::deviationTiltPhaseN(-1.31, -0.16, 0.29, -0.19, -0.14));
	EXPECT_EIGEQQ_UT(Quat(0.9088782267303330, -0.2548603868974291, -0.3301311135988518, -0.0000000001652010), TPMHelper::deviationTiltPhaseN(0.12, 1.02, -0.3, 0.35, -0.02));
	EXPECT_EIGEQQ_UT(Quat(0.7810439832514131, 0.5058297462566980, 0.3662056307986689, -0.0000000007856064), TPMHelper::deviationTiltPhaseN(-0.6, -1.24, 0.19, -0.35, 0.13));
	EXPECT_EIGEQQ_UT(Quat(0.7940271585635376, 0.3623338724959828, -0.4880932659908138, -0.0000000000014088), TPMHelper::deviationTiltPhaseN(-1.03, 0.86, -0.17, -0.07, 0.12));
	EXPECT_EIGEQQ_UT(Quat(0.7750708937303130, -0.4017907530118038, 0.4876774553803994, -0.0000000002751476), TPMHelper::deviationTiltPhaseN(0.97, -0.91, 0.03, 0.06, -0.14));
	EXPECT_EIGEQQ_UT(Quat(0.8064104743361101, -0.1724044177525555, -0.5656667425440637, 0.0000000000139070), TPMHelper::deviationTiltPhaseN(0.38, 1.11, 0.01, -0.1, -0.04));
	EXPECT_EIGEQQ_UT(Quat(0.7476565199923109, -0.6635795051600171, 0.0259223541479978, 0.0000000001814329), TPMHelper::deviationTiltPhaseN(1.29, -0.22, -0.17, -0.22, -0.14));
}

// Test worker function
void testSwingGroundPlane(double phaseX, double phaseY, double expectedPhaseX, double expectedPhaseY, double fusedPitchN, double PNSx, double PNSy, double fusedPitchS, double fusedRollS)
{
	// Perform the swing ground plane computation
	TiltPhase2D PNSRet = TPMHelper::swingGroundPlaneN(phaseX, phaseY, expectedPhaseX, expectedPhaseY, fusedPitchN);
	double fusedPitchSRet = NAN, fusedRollSRet = NAN;
	TPMHelper::swingGroundPlaneAction(PNSRet, fusedPitchN, fusedPitchSRet, fusedRollSRet);

	// Check everything went as expected
	EXPECT_NEAR(PNSx, PNSRet.px, 3e-14);
	EXPECT_NEAR(PNSy, PNSRet.py, 3e-14);
	EXPECT_NEAR(fusedPitchS, fusedPitchSRet, 3e-14);
	EXPECT_NEAR(fusedRollS, fusedRollSRet, 3e-14);
}

// Test: swingGroundPlaneN, swingGroundPlaneAction
TEST(TiltPhaseModelTest, testSwingGroundPlane)
{
	// Test special cases
	testSwingGroundPlane(0.02, -0.19, 0.02, -0.19, 0.16, 0.0, 0.0, 0.16, 0.0);
	testSwingGroundPlane(0.19, 0.35, 0.19, 0.35, -0.02, 0.0, 0.0, -0.02, 0.0);
	testSwingGroundPlane(-0.29, 0.9, -0.16, 0.14, 0, 0.1186389733054178, -0.7594721970915153, 0.7571584778457295, -0.1075028424652214);
	testSwingGroundPlane(-0.76, 1.14, -0.21, -0.11, 0, 0.4586391910271073, -1.2708024456797966, 1.1629898443789486, -0.3376899808530498);

	// Test many different other cases
	testSwingGroundPlane(0.47, 1.4, 0.05, -0.05, -0.04, -0.4056831752631340, -1.4534323817652022, 1.2803120160120716, 0.2716602631849681);
	testSwingGroundPlane(-0.56, 0.67, -0.09, -0.2, -0.05, 0.4508056582574351, -0.8762151424977043, 0.7932009363052929, -0.3912161005603573);
	testSwingGroundPlane(0.68, 0.24, -0.14, -0.35, -0.03, -0.7946470644352337, -0.6153196497308085, 0.5242083900793636, 0.7308014227670807);
	testSwingGroundPlane(-1.13, -0.78, -0.17, 0.34, -0.1, 0.6949898117461382, 1.2313309903182905, -1.0581831120023368, -0.5069309486672656);
	testSwingGroundPlane(1.11, 0.22, 0.08, -0.23, 0.01, -0.9893306610101236, -0.5043679875759230, 0.4239190052150414, 0.9242543672850572);
	testSwingGroundPlane(-0.86, 1.05, 0.35, 0.01, 0.14, 1.2079791132501072, -1.0359115312796547, 0.6965861648030010, -0.8616853304026587);
	testSwingGroundPlane(-0.11, -0.2, -0.23, 0.02, 0.19, -0.1176161457974942, 0.2159446682974404, -0.0267533094531011, 0.1166991357270520);
	testSwingGroundPlane(0.66, -0.83, 0.3, -0.13, -0.18, -0.2990245312141380, 0.7112160963112790, -0.8643633391155237, 0.2736314985766022);
	testSwingGroundPlane(-0.71, 0.58, 0.3, 0.15, 0.13, 1.0052401896872527, -0.4331454961822125, 0.4204227034166250, -0.9547934672379452);
	testSwingGroundPlane(-1.1, 0.43, -0.25, -0.03, -0.1, 0.8613028606171538, -0.4362204834055297, 0.3182197631833150, -0.8235965128735707);
	testSwingGroundPlane(-0.68, -0.44, -0.08, -0.09, -0.12, 0.5849297686170579, 0.3697131706762402, -0.4451926912998525, -0.5696701599336460);
	testSwingGroundPlane(1.05, 0.79, -0.12, 0.31, -0.16, -1.1600817298060082, -0.2633462365949112, 0.1440855395472593, 1.1318241035881567);
	testSwingGroundPlane(-0.66, 1.15, -0.23, 0.13, -0.03, 0.4097821519948178, -1.0240373651230938, 0.9523371981102082, -0.3380032428808852);
	testSwingGroundPlane(-0.16, 1.19, -0.25, 0.28, 0.13, -0.1293393631287886, -0.8945714647402526, 1.0183987006366444, 0.1126717587954413);
	testSwingGroundPlane(0.05, 0.2, 0.06, 0.23, -0.11, 0.0098743060147078, 0.0300112093108693, -0.1400053159949809, -0.0098728237707094);
	testSwingGroundPlane(-1.05, -0.5, -0.14, 0.23, 0.04, 0.8731907326811708, 0.7579090811187069, -0.6228893263278550, -0.7631895763772487);
	testSwingGroundPlane(-1.37, 0.1, -0.28, -0.14, 0.12, 1.0292645696348210, -0.3468101861079309, 0.3429628161323700, -0.9945367436062237);
	testSwingGroundPlane(-0.05, -1.08, 0.35, -0.11, -0.18, 0.4528849842839285, 0.9310797525435659, -1.0202944938169740, -0.3856878721042690);
	testSwingGroundPlane(-0.4, 0.76, 0.13, -0.21, -0.17, 0.5718102668594186, -0.9403879008611297, 0.7397184532948959, -0.4815628880299141);
	testSwingGroundPlane(0.16, -1.56, -0.09, -0.18, -0.15, -0.2746346532710252, 1.3734056810222741, -1.3749244906235034, 0.1944744849280271);
	testSwingGroundPlane(-1.27, -0.73, -0.31, -0.33, -0.18, 0.9246327398836288, 0.4655072787475331, -0.4913626445543245, -0.8758891934844266);
	testSwingGroundPlane(-0.88, 0.14, 0.08, 0.14, 0.16, 0.9535341561616111, -0.0534960149279782, 0.1375956637363088, -0.9528180151535208);
	testSwingGroundPlane(-1.04, 0.59, 0.25, -0.08, -0.2, 1.3470390366639551, -0.4445261946011335, 0.2769633420481932, -1.2186513971004356);
	testSwingGroundPlane(0.73, 1.33, 0.19, 0.25, -0.08, -0.5564897478762976, -1.0713467181547369, 0.9245592678608358, 0.4454103108188118);
	testSwingGroundPlane(1.02, -0.82, 0.31, -0.09, -0.08, -0.6582029134397662, 0.7624430552670951, -0.7485093172045174, 0.5852617189286037);
}

// Test: Misc
TEST(TiltPhaseModelTest, testMisc)
{
	// Test Eigen
	Vec2 vec(0.7, -0.4);
	EXPECT_EQ(0.7, vec.x());
	EXPECT_EQ(-0.4, vec.y());
	vec << -vec.y(), vec.x(); // Note: Careful of this pitfall => vec.x() gets assigned the value -vec.y() before its value is used to assign vec.y()!
	EXPECT_EQ(0.4, vec.x());
	EXPECT_EQ(0.4, vec.y());
}

// Main function
int main(int argc, char **argv)
{
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}
// EOF